#include "CD4051.h"
#include "SGM51242.h"

// 初始化函数
void ADG_Init(void) {
    RCC_APB2PeriphClockCmd(ADG_GPIO_CLK, ENABLE);

    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = ADG1_A0_PIN | ADG1_A1_PIN | ADG1_A2_PIN |
                                  ADG0_A0_PIN | ADG0_A1_PIN | ADG0_A2_PIN;
    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_PP;   // 推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(ADG_PORT, &GPIO_InitStructure);
    GPIO_ResetBits(ADG_PORT, ADG1_A0_PIN | ADG1_A1_PIN | ADG1_A2_PIN);
    GPIO_ResetBits(ADG_PORT, ADG0_A0_PIN | ADG0_A1_PIN | ADG0_A2_PIN);
}
// 选择通道：0~7
void ADG_SelectChannel(uint8_t index, uint8_t ch) {
    if (ch > 7) {
        return;
    }
    uint8_t inverted_ch = ~ch & 0x07;   // 反相地址

    if (index == 1) {
        GPIO_ResetBits(ADG_PORT, ADG1_A0_PIN | ADG1_A1_PIN | ADG1_A2_PIN);
        if (inverted_ch & 1) {
            GPIO_SetBits(ADG_PORT, ADG1_A0_PIN);
        }
        if (inverted_ch & 2) {
            GPIO_SetBits(ADG_PORT, ADG1_A1_PIN);
        }
        if (inverted_ch & 4) {
            GPIO_SetBits(ADG_PORT, ADG1_A2_PIN);
        }
    } else {
        GPIO_ResetBits(ADG_PORT, ADG0_A0_PIN | ADG0_A1_PIN | ADG0_A2_PIN);
        if (inverted_ch & 1) {
            GPIO_SetBits(ADG_PORT, ADG0_A0_PIN);
        }
        if (inverted_ch & 2) {
            GPIO_SetBits(ADG_PORT, ADG0_A1_PIN);
        }
        if (inverted_ch & 4) {
            GPIO_SetBits(ADG_PORT, ADG0_A2_PIN);
        }
    }
    delay_us(100);
}
/*滤波CD4051*/
uint16_t       CD4051[CHIP_NUM][CH_NUM];
static u16     cd1_fifo[CH_NUM][SAMPLES_NUM] = {0};   // FIFO 存储 5 次采样的数据
static u16     cd0_fifo[CH_NUM][SAMPLES_NUM] = {0};   // FIFO 存储 5 次采样的数据
static int     cd_fifo_head[CHIP_NUM]        = {0};   // FIFO 写入指针
static uint8_t cd_count[CHIP_NUM]            = {0};

void CD4051_proc(uint8_t index) {
    cd_count[index]++;
    if (cd_count[index] < 50) {
        return;
    }
    u16 channel_data[CH_NUM] = {0};

    for (uint8_t i = 0; i < CH_NUM; i++) {   // 数据填充
        ADG_SelectChannel(index, i);
        if (index == 1) {
            channel_data[i] = Get_adc_origin(6);   // CD4051输出到SGM512的IN6 index=1,IN6. index=0,IN7
        } else {
            channel_data[i] = Get_adc_origin(7);   // CD4051输出到SGM512的IN7
        }
    }
    for (int i = 0; i < CH_NUM; i++) {   // 滤波
        cd_fifo_enqueue(index, (channel_data[i] & 0x0FFF), i);
        if (index == 1) {
            CD4051[index][i] = Cd_Aver_Filter_u16(index, cd1_fifo[i], SAMPLES_NUM);
        } else {
            CD4051[index][i] = Cd_Aver_Filter_u16(index, cd0_fifo[i], SAMPLES_NUM);
        }
    }
    cd_fifo_head[index] = (cd_fifo_head[index] + 1) % SAMPLES_NUM;
    cd_count[index]     = 0;
}

// 更新 FIFO 数据
void cd_fifo_enqueue(uint8_t index, u16 data, u16 count) {
    static u8 fifo_count[CHIP_NUM] = {0};
    if (index == 1) {
        if (fifo_count[1] < CH_NUM) {
            for (int i = 0; i < SAMPLES_NUM; i++) {
                cd1_fifo[count][i] = data;
            }
            fifo_count[1]++;
        } else {
            cd1_fifo[count][cd_fifo_head[1]] = data;
        }
    } else {
        if (fifo_count[0] < CH_NUM) {
            for (int i = 0; i < SAMPLES_NUM; i++) {
                cd0_fifo[count][i] = data;
            }
            fifo_count[0]++;
        } else {
            cd0_fifo[count][cd_fifo_head[0]] = data;
        }
    }
}
static u16 cd_filter_buffer[CHIP_NUM][SAMPLES_NUM];

u16 Cd_Aver_Filter_u16(uint8_t index, u16* buffer, INT8U num) {
    INT8U i, j;
    u32   temp_int;

    if (num > SAMPLES_NUM) {
        return 0;
    }

    for (i = 0; i < num; i++) {
        cd_filter_buffer[index][i] = buffer[i];
    }

    // 中位值平均滤波法
    for (j = 0; j < num - 1; j++) {
        for (i = 0; i < num - j - 1; i++) {
            if (cd_filter_buffer[index][i] > cd_filter_buffer[index][i + 1]) {
                temp_int                       = cd_filter_buffer[index][i];
                cd_filter_buffer[index][i]     = cd_filter_buffer[index][i + 1];
                cd_filter_buffer[index][i + 1] = temp_int;
            }
        }
    }

    temp_int = 0;
    for (i = 1; i < num - 1; i++) {
        temp_int += cd_filter_buffer[index][i];
    }

    return (temp_int / (num - 2));
}
/*获取CD4051的AD值*/
uint16_t Get_CD4051(uint8_t index, uint8_t ch) {
    if (ch > CH_NUM) {
        return 0;
    }
    return CD4051[index][ch];
}
